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ABSTRACT 


An integrated model of acoustic motion estimation and control is presented. The 
control system is designed on the basis of the definitions of suitable Lyapunov functions 
for the different maneuvers in approaching a target. These allow the navigation and 
maneuvering tasks to be performed in a two-layered hierarchical architecture for closed- 
loop control. The motion estimation algorithm uses pencil beam profiling sonar range and 
bearing information. The operating environment is modeled with a suitable three- 
dimensional potential function and its gradient which forms an attractive field. This 
algorithm provides satisfactory performance for autonomous navigation and obstacle 
avoidance. 

The applicability and robustness of this model are demonstrated with both actual 
test data obtained with the NPS Phoenix submersible and computer generated simulation 
data. The results show the effectiveness of the combined estimation and control 


techniques. 
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I. INTRODUCTION 


A. GOALS 

An Autonomous Underwater Vehicle (AUV) is a self-contained unmanned vehicle 
used for underwater missions such as surveying, pollution detection, or mine detection and 
neutralization. In order to be deemed truly autonomous the vehicle must be able to 
independently follow a mission plan, interact with its environment, accomplish a goal and 
return to a predesignated destination. 

This thesis addresses the problem of guidance and control of autonomous vehicles. 
In particular, we will be focusing on the underwater applications, with the aid of sonar and 
inertial sensors to detect features in the environment. We assume the environment to be 
unknown at the beginning of the mission, and the vehicle must be capable of detecting 
and reconstructing possible obstacles. Then, the next step is to estimate the motion of the 
vehicle in this environment and keep the vehicle on the desired trajectory by applying a 
closed-loop guidance and control system. | 
B. METHOD OF APPROACH 

In this study, a guidance and control system based on a two layered hierarchical 
architecture for closed-loop control has been integrated with an acoustic motion estimator 
designed for navigation in structured environments. The computer simulations of both the 
control system and the estimator are presented together with the combined system. 

The guidance and control system design is based on the approach which tackles 
the problems of using control laws based on Lyapunov functions. These allow the 
navigation and maneuvering tasks to be performed in a closed-loop in the presence of 
perturbations and modeling errors. The simulation results showing the possible 
effectiveness of this closed-loop system are also provided. 

In the motion estimation part, we address the problem of a vehicle navigating both 
in an environment which could be entirely or partially known [Ref. 1]. The assumption on 
the obstacles is that their reflective surfaces are piecewise modeled by segments having 


constant orientation in two dimensions. We assume the vehicle's initial location and 
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velocity are known, and we measure its acceleration. The goal of the algorithm is to 
provide the best estimate of the vehicle's motion and of the segments of the environment, 
in terms of location and orientation. The environment is modeled by a potential function 
based on the estimated location of the reflecting surfaces and their orientation. Then, this 
potential function estimate of the environment is used to correct the vehicle's estimated 
trajectory. By the potential function approach, through the range and bearing 
measurements from a scanning sonar, the motion trajectory of the vehicle is estimated 
using Kalman filtering techniques. The simulation results and program codes are 
presented. 

Finally, the combination of two systems yields a system of acoustic motion 
estimation and control. The measurements from a scanning sonar are recursively 
processed in the estimation algorithm, and the result is an estimate of the current position 
of the vehicle. The estimated vehicle's position is processed in the control algorithm to 
make it follow a desired trajectory. 

c. ORGANIZATION 

This thesis is organized in five parts. Chapter II provides the theory behind the 
design of the vehicle and the environment models, as well as the interaction between the 
two, provided by the sonar system. The Kalman filter-based algorithm which is used to 
estimate the motion of the vehicle, is detailed. The simulation results due to this approach 
are also included. 

Chapter III deals with the guidance and control algorithms based on the Lyapunov 
control techniques. The control algorithm which includes two loops: The outer control 
loop which corresponds to three phases of the vehicle approach for the target, and the 
inner control loop which is determined by the vehicle's dynamics, is detailed. The 
simulation results using MATLAB SIMULINK software package are also included. 

Chapter IV is based on the combined model of the vehicle motion estimation and 
control system. After giving some brief definitions, the MATLAB SIMULINK model of 


the design is detailed and the results are presented. 








Chapter V summarize the results and conclusions of this study and provides 


several recommendations for follow-on research. 











Il. ACOUSTIC MOTION ESTIMATION MODELING FOR AN AUV_ 
IN A STRUCTURED ENVIRONMENT 


A. GENERAL 

In this chapter, a nonlinear model for the dynamics of an AUV and the 
environment in which it operates, is set up. Basically, it is intended to determine a 
framework, so that we can combine inputs from the inertial navigation system (INS - 
gyros, accelerometers) with sonar for the localization of the vehicle with respect to the 
known initial conditions (position and velocity). A mission in which the vehicle explores 
the surroundings guided by its INS equipment while it maps the environment, is 
performed. 
B. ENVIRONMENT MODEL 

We define environment to be the locus E of reflecting surfaces [Ref. 1]. In order to 
be able to estimate the vehicle's motion, it is assumed that a certain continuity and 
smoothness conditions are satisfied, at least in a piecewise sense. Therefore at every point, 
where the sonar beam pings on the reflecting surface E, a vector », orthogonal to the 


surface itself, is defined. This can be expressed as 


9 (q) q=l, Q.1) 
where q is the tip of the sonar vector on the reflecting surface. 

By this, it is assumed that the origin of the coordinate frame does not belong to the 
reflecting surfaces. For example, the origin is assumed to be the initial location of the 
vehicle, which is the assumption for beginning its mission in open waters. 

Assuming that the sonar head continuously scans the environment, if the vehicle 
moves smoothly, the point q on the reflecting surface and the vector @, normal to the 


reflecting surface, can be related in a state space equation as 


re = 0, t¢eS 
(2. 2) 


p(t) q=1 





with %, denoting the set of times when the slope vector ® changes. Clearly, at every 


point of the reflecting surfaces, the vector , is orthogonal to the surface. 

At this point the normal vector @ can be expressed as the gradient of a potential 
function V [Ref. 1]. The potential function V will be defined over the whole operational 
space, satisfying the following necessary conditions: 

e V(q)20, forall q; 

e V(q)=0, if and only if the point q belongs to the reflecting surface defined 

on the map; 

e For any small perturbation Aq, the gradient of V(q) is such that 
0>Aq'VV(q+Aq), which states that the vector field is attractive towards the 
reflecting surface, and also its gradient is orthogonal to this surface. 

The critical point is the last property above, which shows that the two vectors and VV 
are parallel. The reason behind the orhogonality of the gradient has been explained in 
Ref. 1, and will be recalled later in the chapter. 

The sonar returns produced by the sonar installed on the AUV, provide the 
required interaction between the AUV dynamic model and the environment, through the 
definition of the potential function. A summary of the operating characteristics of the 
high frequency scanning sonar installed on the NPS Phoenix AUV is included as 


Appendix A and a typical potential function for a rectangular pool is presented as Figure 


2.1 and Figure 2.2. 
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Figure 2.1 Three Dimensional Potential Function of a Rectangular Pool (24x24 ft). 
Figure 2.2 The Contours of the Potential Function of a Rectangular Pool(24x24 ft). 








C. VEHICLE MODEL 

The dynamics of the vehicle moving in an environment is represented by a state 
vector determined by its position vector p with respect to a reference frame fixed with the 
environment, a velocity and orientation vector z, and a command vector u, involving the 
commands given to the actuators of the vehicle. The dynamics can be expressed as a 


combination of the kinematics and the dynamic equations of the form [Ref. 1], 


(2. 3) 


= f, (z), 
z=f,(z,U). 


The mappings represent the kinematics and the dynamics relations respectively. In 
the case of the acceleration being measured by its inertial navigation system (INS), as we 
would use for our purposes, we can write the vehicle's equations using purely the 


kinematics. Denoting a, the measured acceleration vector and v its velocity, this becomes 
p=v, 
(2. 4) 
V=a. 


The main component of the estimation process is based on the dynamic model of 
the vehicle which is defined in equations (2.3) and (2.4). 

The measurement vector p, consists of the dynamic observations of the sonar 
range r, at an angle 0, with respect to the longitudinal axis of the vehicle, with w 


indicating measurement noise [Ref. 2]. 


p=g(p,r,9,w) (2. 5) 


We assume the noise sources w to be zero mean, white, and Gaussian, as is 


standard in this class of problems. 











The function g is defined by the map of the environment, and apart from the 
measurement noise, it is zero when the sonar return information (1,0) is consistent with 
the vehicle position on the map. 

The criterion by which we choose g is based on the use of the potential function 
V, which is defined in the previous section. For a vehicle moving on a plane located on 


the position (x,y) with a heading ¥, the function g is defined as 


g(x, y,¥1,8)=V(x+rCos (y+9),y+rSin (y+ )), (2. 6) 


The vector q, for the location of the tip of the sonar range in the reference frame 


can be expressed as the equations below 


p=[x y]. 
s= [rcos@ rsin@]', 


cosy -—siny (2. 7) 
T= : 


siny cosy 


qg=ptTs 


where p, is again defining the position of the vehicle at any time, s is the sonar 
information vector consisting of the measured range and angle of the sonar return. The 
matrix T represents the coordinate transformation between the vehicle and the 
environment reference frames. 

In order to design an algorithm for the estimation of the vehicle's position from 
sonar measurements, using the definitions in (2.7), the overall state space model 


including the vehicle motion and the environment is considered as 


p=V, 
v=a, (2. 8) 
O= V(p+Ts), 





Defining the estimated position of the vehicle as p, for the general case where 


acceleration measurement is available with an inertial navigation which includes 
accelerometers, the correction can be applied to the position and velocity estimate [Ref.3]. 


These can be updated as 


p=¥ +p, VV(p+Ts), 
(2. 9) 
vV=at+p,VV(p+Ts), 


with L1; and LU, being positive constants. 
To demonstrate local convergence of this method, let us consider a simpler 


problem where the velocity vector v is measured. Defining the position error as p = p-p, 


and combining equations (2.8) and (2.9), we obtain the error equation, 


p =yVV(pt+Ts), (2. 10) 
and pre-multiplying by the transpose of the error yields, 
p'p=up'VV(p+Ts+Pp), (2. 11) 


where we recognize that the point p+Ts is on the reflecting surface and that for p being 
sufficiently small, the right-hand side of the above equation is non-positive by the property 


specified for the potential function. From (2.11) we can see that pl’ is decreasing with 


time since < tol =-2p'p<0 forallp+0. 
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Integration of the equation (2.11) with respect to time yields, 


= def -[po/ - 

t (2. 12) 

pf p(t)” VV (p+ Ts+ p(t) dt 
0 


Since the integrand is non-positive for all t, it will go to zero as time tends to infinity. 


This implies that the error vector p tends to be orthogonal to the gradient of the field, and 
that the point p+Ts tends to be on the reflecting surface. 


A particular form of the gradient is obtained by defining it as 
VV(p+Ts+p) =—-00' Dp, (2. 13) 


where, @ is the orthogonal vector to the reflecting surface. The mathematical verification 
or the proof of convergence in the special case of acceleration vice velocity measurements 
is addressed in Ref. 1. 

D. COMBINED MOTION AND ENVIRONMENT MODEL 

The most important part of this research is to be able to identify the environment 
in a dynamic fashion, by continuously updating the mathematical model (the potential 
function). Ideally, we may start with an initial estimate of the environment and refine it 
during the mission by including the data received from the sonar returns. The 
identification of the new objects, not present in the local map would be the outcome of 
the mission. 

Firstly, from the known initial position of the vehicle, we build an initial estimate 
of the environment for our task purposes. The operational area is divided into occupancy 
cells, as described in Ref. 1. The number and the size of these cells is defined before the 
construction of the potential function model of the environment. Then the sonar data, 
using the initial position as a reference, is analyzed within each cell. In this approach, 


within each cell &; , the reflecting surface is modeled by an ellipsoid defined by its center 


11 








vector cj and covariance matrix Pj, as it were a Gaussian distribution. These parameters 


are computed as 


Cy = » gy PCO + Ts(t), 
: (2. 14) 
P, = 2, gy (P(t) + Ts(t)—¢; ) (p(t) + Ts(t) -¢, yt 


where Xqj) restricts the summation to terms belonging to the cell € qj) . In the cell & a 


where a return is detected, the potential function is constructed as 


Vi (@=(q-ey) Pix G-ey) forac§ w. (2. 15) 


This is consistent with the definition of the potential function on the reflecting surfaces in 
(2.8). In the cells where no return is detected, the nearest cell containing a reflecting 


surface is determined. 


After initially constructing the environment and defining the initial state, we move 


on to the task of motion estimation. 


The general structure of the sensor based navigation system is shown in Figure 2.3 


[Ref. 1]. 


Z S Z S a Zz S 


Feature Extraction Environment Model 


Decision 


Figure 2. 3 General Structure for Combined Motion and Model of the Environment. 
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We consider the case of navigating the vehicle within an unknown area using its 
inertial navigation systems (INS), while collecting data from the scanning sonar. The 
model of the environment built in this way is subsequently used to correct for the errors in 
the INS and provide a current estimate of the vehicle's position. 

The block ‘feature extraction’ estimates relevant features of the environment, that 
will be used to either validate or reject the potential function of the environment. While 
collecting data from the scanning sonar, we are constructing the potential function model 
of the environment, to be able to compare and/or update the initially estimated model. In 
the ‘feature extraction’ block, the slope of the environment is estimated by using the 
measured sonar data. As in (2.14), this time using the estimated vehicle position and 
velocity, we obtain the centroid vector and covariance matrix of the measured data for the 
time interval. It is considered that these reflecting cells are ellipsoids and are modeled as 
Gaussian distributions. Since the covariance matrix P; is Hermitian, in our case where we 
have a two dimensional problem, we can compute the two eigenvalues Amax and Amin, and 


the two corresponding orthonormal eigenvectors, emax and €min, Satisfying the relation 


Py en =Ax Ck 5 k=1.2 (2. 16) 


Typical contours of a Gaussian density function are ellipses. If we consider one of 
the ellipses, it is centered at the coordinates of the mean vector, (centroid vector) and 
oriented with its axes parallel to the direction of the eigenvectors. The major and minor 
axes are proportional to the corresponding maximum and minimum eigenvalues, 
respectively. The slope of this ellipse is the slope of the major axis, which is the 
eigenvector corresponding to the maximum eigenvalue. In our case, the eigenvector 
corresponding to the maximum eigenvalue of each covariance matrix Pj , gives the slope 
of the reflecting cell. This information of the measured data is going to be used in the 


‘decision block’. 


The ‘decision block' uses the slope information to validate the current mode! of the 


environment by either discarding the measurements which show inconsistencies with the 
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model, or/and, by updating the mathematical model itself with new segments. Ideally, 


when the model and the measurements are consistent with each other, then this relation 
holds 


A, if k=m 
om P; en = (2. 17) 
0 if k#m 


where k and m are the specified times and 1, is the maximum eigenvalue of the covariance 
matrix of the initially predicted environment cell. This information is going to be used im 
the ‘estimation block’ , in order to estimate the position and the velocity of the vehicle 


from the data and the potential function (model of the environment). 


Although the overall above system is not observable, assuming we know the imitial 
conditions of the vehicle in terms of initial position and velocity, these can be applied to 
the Extended Kalman Filter (EKF) to estimate the state vector p. 

The Kalman Filter estimates the state of a system given a set of known inputs to 
the system and a set of measurements. [Ref. 4]. The system is assumed to be driven by 


both a known input and an unknown random input, in our case 


p(k+1) =A p(k) + Ba (k) + Ai w &&), (2. 18) 


where p(k) is the state vector (position and velocity of the vehicle), A is the known state 
transition matrix, B is the known input matrix, a is the known acceleration vector, and A; 
and w(k) are the unknown, random input matrix and vectors. We assume w(k) is white 


noise. The measurements of the system are related to the state by 


q (kK) =Hp (kK) +7 W), (2. 19) 


where q (k) is the measurement, H is the measurement matrix , and j (k) is the random 


noise. We assume that 7 (k) is white noise. 


14 














The solution of the estimation problem can be shown to have the following form 
p(k +1k+1) = p(k + Ik) +K(k+Diqk +1) -q(k+)), (2. 20) 


where K (k+1) is the Kalman Filter gain at each time index (k+1). 

The form of the Extended Kalman filter equations are essentially similar to those of 
the Linear Kalman filter. This can find applications in nonlinear systems. The nonlinear 
model is used to predict the state and the nonlinear measurement is used to correct the 
prediction. The details and the Extended Kalman Filter Equations can be found in [Ref. 4]. 

In using the Extended Kalman Filter, the nonlinearities are modeled as plant noise. 
This means A; matrix in (2.19) is usually the identity matrix. Furthermore, because the 
nonlinear effects are not included in the gain matrix, the Extended Kalman Filter can be 
very sensitive to the W and 1 matrixes. In our case, considering 1 is the measurement 
noise of the sensors, we can include the inconsistencies of the measured slope to the 
model to this term in the equations. As with the measurement noise covariance matrix, if N 
is large, then the measurements are expected to deviate more from the states being 
measured, and the Kalman Filter rely more on the predicted state than on the 
measurements. [Ref. 2]. So, if the slope of the environment obtained from measurement 
data is inconsistent with the model of the vehicle's position and velocity, the estimates are 
based on the previous estimates and the model only. 

In order to attempt to update dynamically the map of the environment, it 1s 
important to have a criterion to establish whether a sonar return comes from a mapped or 
unmapped object. 

By standard Kalman Filtering techniques an estimate of the state p(t), v(t) and its 
covariance matrix can be obtained. From [Ref. 1], denoting W(t) the covariance matrix 
relative to the estimated position error, a likelihood function on the consistency of the 


sonar return at time t given all past measurements, can be defined. 
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In particular, assuming Gaussian errors, this would be given by the expression 


L(t) =In Pr {s(t)| s(t—1),...,s(0)} = 
Vit)’ (2. 21) 


] T z ee 
C- 7 ne® WHeO-> CHT wie) 


with Pr indicating the probability, C is a constant and the terms V and its gradient @ can 
be computed using the estimated vehicle position. A threshold can be established, by 
which low probability status can be assigned for the unmapped object, most likely by trial 
and error basis. 

This model is suitable where the environment is composed of surfaces with a 
piecewise constant orientation. This would be the case of the vehicle operating in an area 
of walls or manmade structures. 

E. APPLICATIONS 

In this section, we address the problem of applying the combined model of the 
vehicle motion and the potential function approach for the environment. Although we will 
be using the model of the test tank at the NPS Annex, the results can be generalized to 
more complex environments, such as harbors, pipelines, etc. The rectangular environment 
with closed borders is the simplest realistic operating area which can be modeled with the 
potential function approach and would reflect operations such as acoustic test data- 
gathering with the NPS Phoenix AUV in the laboratory pool. 

The program codes which are written in C for simulation purposes, are included in 
Appendix B. The program is using real data, that is collected by the AUV during pool 
testing. Although these programs are tested in simulations, they may be embedded with 
the original program of the vehicle, and used in real time applications. 

In the first set of the simulation we are going to use the real time data collected in 
the test pool at NPS. For here we assume that the environment is partially known. The 
operational area, in our case the sizes and the shape of the pool is known, however the 


initial conditions (position and velocity) with respect to the fixed reference and the 


presence of two unmarked cylinders are not known. 
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The environment, we are testing is the pool at the AUV lab. at NPS. Figure 2.4 


shows the contours of the potential function of the pool and the two cylinders which have 
been placed for testing purposes. 
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Figure 2. 4 The Contour Plot of The Pool with Two Cylinders. 


A set of sonar data (range and bearing) has been collected while the vehicle is 
moving at a constant rate. The Cartesian plot of the data is given in Figure 2.5, which 


shows a collection of six consecutive scans. The scans have been taken 0.9 degrees apart. 
That results in 400 data points per scan. 
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The data in Cartesian coordinates can be seen in Figure 2.5. 
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Figure 2.5 The Data in Cartesian Coordinates. 


At the beginning of the run, the vehicle does not know the environment, and it has 
to build a map in terms of the potential function. The vehicle is moving, but the velocity of 
the vehicle is not known, and the initial position of the vehicle is arbitrarily considered at 
the middle of the pool. The orientation and the distance of each wall is computed by 
applying the slope algorithm (as previously discussed) to the walls of the pool. Figure 2.6 
shows the contours of the estimated environment where we can see both the walls of the 
pool and the effect of the two obstacles. The data at the bottom right corner are due to 
spurious returns of sonar, and can be easily filtered. These do not belong to the 


environment. 
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Figure 2.6 The Contours of the Estimated Environment. 


In Figure 2.7, we show the estimated trajectory of the vehicle, since we do not 


have the exact trajectory available. 
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Figure 2. 7 The Trajectory Estimation of the Vehicle. 
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We show the validity of the estimate by comparing the estimated locations of the 
walls of the pool with the actual ones. We can see that the two coincide reasonably to 


infer that the estimated trajectory is close to the actual trajectory. This is shown in Figure 
2.8. 
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Figure 2. 8 The Estimated Environment with the Actual Environment. 


In the second set of the simulations, we are going to use a sonar data set, created 
by using known position and velocity of the vehicle. This time we can see the consistency 
of the real trajectory of the vehicle with the estimated one. 

The vehicle is assumed to be moving from the initial position coordinates (0,0), to 
the coordinates (6,6), in a same kind of environment (6 x 6 m rectangular pool), without 


any obstacle presence. The vehicle velocity is set to be about 0.024 m/sec. 
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In Figure 2.6, the motion of the vehicle in the x-direction is shown. 
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Figure 2.9 The Trajectory of the Vehicle in the X-axis. 


Then, the motion of the vehicle in the y-direction is shown in Figure 2.7. 
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Figure 2.10 The Trajectory of the Vehicle in the Y-axis. 
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Finally, the whole trajectory of the vehicle and the estimated value for its velocity 
can be seen in Figure 2.8 and Figure 2.9, respectively 


y(m) 


. 
Pe ee es 


» 

eae ae ee eee ee ee ee, ee ee id ee © eee 
. > 
« 
. . 
. . 

. . 
. 





2 3 4 
x(m) 


Figure 2.11 The Trajectory of the Vehicle. 
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Ill. LYAPUNOV BASED CLOSED-LOOP MOTION CONTROL 
FOR AN AUV 


A. GENERAL 

In this chapter, the problems of navigation, guidance and control of the AUV are 
addressed. In particular, these problems can be summarized as reaching a target frame 
(position and orientation) and remaining stationary on site, even in the presence of 
disturbances. It is intended to use the control laws based on the definition of suitable 
Lyapunov functions [Ref. 5], in order to allow the navigation and maneuvering tasks to be 
performed in a closed-loop without requiring any off-line preplanning.[Ref. 6] 

B. SYSTEM MODELS 

The kind of task to be performed governs the choice of the set of the coordinates 
which will describe the position of a frame fixed on the vehicle with respect to the target 
frame. 

In general for an underwater vehicle, the state vector is not only composed of the 
linear longitudinal velocity » and the angular velocity @ (which directly affects vehicle 
rotation), but also of the lateral velocity v. The control variables refer to the orthogonal 
reference frame centered on the vehicle itself. The first set of kinematics equations can be 


directly obtained from Figure 3.1 [Ref. 7]. 


Vehicle 





Figure 3. 1 System Coordinates. 
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In Figure 3.1, the vehicle's position x, y and the orientation angle 9, are all 


measured with respect to the target frame center O, then the first set of equations are 


x=vVcos > - Vsingo 
y=vsind +Vvcos o (3. 1) 


b= 


where ‘v,v and w are the speeds defined above. 

The second set defines the state vector as [ e @ 0], which is chosen and assumed 
to be completely measurable at any time. The variable e represents the distance error from 
the target frame, m denotes the angle between advanced orientation and the vector e while 
© is the angle between advanced orientation and the x axis of the target frame. 

From their definitions and the geometry in Figure 3.1, the following equations can 


be derived 


é=—vcosa—Vsina 


. vSsina vcosa 
pS 
e e 


(3. 2) 


Vsingd Vcosa 
e e 


&=-0+8=-O+ 


It should be noted that this set of equations is only valid for non-zero value of the 
distance errors e, otherwise, both angles « and & simply would be undefined quantities. 

Both models (3.1) and (3.2) are kinematics, so the linear (V,v) and angular (@) 
velocities need to be imposed. To these ends, it is necessary to use a dynamic system 
[Ref. 7]. Let us assume that all the masses and inertial moments are equal to the unitary 


value. New variables describing the forces and moments are introduced. 
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In this case, the dynamics of the vehicle becomes 


v=f+vo 
V=g+00 (3. 3) 
o®=M 


where f and g are the forces of the thrusts of the frontal and lateral screw propeller and M 


is the angular moment. 
C. CONTROL ARCHITECTURE 
The control loop is designed based on the approach described in Ref. 6. This is 


set as two layered hierarchical control architecture seen in Figure 3.2. 


Outer Control |v v @ | Inner Control 
Loop 





Figure 3. 2 Two Layered Hierarchical Control Loop. 


Defining the Lyapunov function V= V(z(t)), with z is the state [Ref. 6], the 
control law is designed by imposing V<0. The particular operational variables of 
motion are designated for each task, and the outer loop eliminates any operational error 
by computing the reference velocities. These will be a feedback for the inner control loop 
on the next turn. 

The inner control loop is task independent and implements a velocity servo loop, 
based on the vehicle dynamics. However, the outer control loop lets the reference 


trajectory based on the task to be followed. 
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D. OUTER CONTROL LOOP 


By the outer control loop, we set the desired motion of the vehicle in order to 
perform the given task. As mentioned above, we define different vehicle trajectories to 
satisfy different requirements during the approach to the target. Three main tasks are 
defined as the maneuvers of the vehicle [Ref. 6]. 

1. Long Range Navigation 

Regardless of its orientation, this task is performed by the vehicle, when it is 
sufficiently distant from the target area. The heading of the vehicle, yaw rate and speed 


can be measured by a gyrocompass and a Doppler sonar. 





Figure 3. 3 Long Range Navigation. 


The Lyapunov function associated to the task of approaching the target and its 


first time derivative can be expressed as [Ref. 6] 


eae =1/2e? =1/2(x’ +y’), 
(3. 4) 


V(x, y)=xxt+y V. 


Now, the problem is to find a control which makes its derivative negative semidefinite. 
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Plugging the values for x and y from (3.1), and collecting the terms, we obtain 


the expression below 


V(x, y) = 0(xcoso+y sino)+ Vv (y cosd—x sin) (3. 5) 


From here we are going to define the reference values for the outer control loop 
variables, linear and angular velocities, and the heading angle, as v,V',@, and o. The 
reference values for v and @, are set to be v'= w = 0 [Ref. 6]. Also, by the fact that the 


vehicle is heading to the target, would be 


* = tan” (—y,—x) (3. 6) 
Then, using the reference variables, (3.5) can be expressed as 
V(x,y) =v (xcos* +y sing’), (3. 7) 


setting D as 
vy =-k,(xcoso+ysing ) (3. 8) 


where v, describes the reference value for the linear velocity, v, and b is the desired 
heading, makes the first time derivative of the Lyapunov function negative definite. 

2. Medium Range Navigation 

At this phase, the vehicle is sufficiently close to the target, and it begins to 
approach with the required orientation. For the simple case, it is assumed that the desired 
orientation is 6 = 0. The state vector [ e « 3], which is defined in Section B, will be used 
to describe the task function. The state variables are shown in Figure 3.1. For this 
operation the following Lyapunov function and its derivative can be defined as [Ref. 6] 
oes, = 1/2 Ae? = 1/2 (a? + hd”), 
; (3. 9) 

V(e,a, 0) = aat+ hdd. 


a7 





In general the lateral velocity v can be either neglected, due to the low term speed, 
or compensated by the lateral thrusters. In this study we assume the lateral velocity to be 
zero, i.e. V = 0. Plugging the values for & and 8 from (3.2), and collecting the terms, 


we obtain the derivative expression as 


cos O 





V(e, a, 9) = —AaM+ ee (a+hd)-—v (a —hd) (3. 10) 


e 
Setting, the reference values for linear and angular velocities, v and @ as in the 
following, and v'=0, we guarantee that the derivative of the Lyapunov function is 


negative definite. 


v” =k,,ecosa 
v =0 (3. 11) 


: cos sin & 
o” =nat+k,, ——_— (a+ hd) 


In these equations 1), km are positive constants. 

3; Short Range Navigation / Hovering 

At this phase, when the vehicle is very close to the target, it hovers above it. The 
vehicle must be fully controllable, so it must be equipped with traverse thrusters as well 
as longitudinal ones. When the area is structured, the position and the speed can be 
estimated by a combination of sonar, visual techniques and its INS. The fine maneuvering 


Lyapunov function [Ref. 6] and its first time derivative can be defined as 


(3. 12) 


wee =1/2A(x? +y?)+1/2h9’, 
V(x, y,6) = A(xk + yy) + hoo. 
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Plugging the values for x,y and from (3.1), and collecting the terms, we 


obtain the derivative expression as 
V(x, y,0) =A v (x cosh +y sind) +A v (-x sing + y cos) +h od ® (3. 13) 
Setting, the reference values for linear and angular velocities, vv and @ as in 
the following, we guarantee that the derivative of the Lyapunov function is negative 


definite [Ref. 6]. 


vy’ =-k, (xcoso+y sin 96), k, >0 


v =-k,(—xsind+ycos9), k, >0 (3. 14) 
o”° =-nNd, n>O and k, =k, 


where 1), Ky and k, are positive constants. 


E. INNER CONTROL LOOP 

As described above, the inner control loop structure in Figure 3.3 depends on the 
system dynamics and is independent of the particular task performed. Thus, the dynamic 
equations, (3.3), together with (3.1) and (3.2), globally yields a second order system, with 
the control variables f, g, M as inputs, and either the state vector [x y 6 D Vv @]' for the 
long range and short range navigation phase or the state vector [e a 0 v Vv a)’ for the 
medium range navigation phase as the outputs, as seen in Figure 3.2. The task of the 
internal loop is to follow a desired vector w ‘=[v vw]. Finally, a Lyapunov function 


[Ref. 6] composed of the velocity terms is defined as 


Va=¥edy (v-0 2 +¥%w(v-V P+tRAg(@ wo (3. 15) 
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After differentiating and substituting the expressions for the angular and linear 
velocities in (3.3), 


V; =A, (v—-v'*)(d—-d°) +A, (V-V' )(V-V") +1, (8-0 )(O-@") 
(3. 16) 


=, (v-v" )(£+V@—") +A, (V—-V" )(g—vO-V") +A, (@-@")(M-@") 


the control variables are chosen to force the derivative of the task function to be negative 


definite [Ref. 7]. Then, the control variables can be defined as follows 


f=-v@+0' —p,(v-v ) 
g=vO+V' —p,(Vv-V ) (3. 17) 
M=0'—p,,(@-@ ) 

This choice implies that the error on velocity tends toward zero (Aw 0). 


The coefficients pp, pg, Pm are the gains of the proportional part and are free 


parameters that can be tuned to achieve optimum performance. 
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F. SIMULATION RESULTS 

All simulations were based on the hydrodynamic model explained in previous 
sections. The scenario is designed by a MATLAB SIMULINK model shown in Figure 
3.4. | 


state 
vectors 





Figure 3. 4 SIMULINK Model of the Control Algorithm. 


The corresponding MATLAB files used in above model are presented in 
Appendix C. 

Jn this simulation the target is positioned in (10,10) with the final required 
orientation of 0 degrees. The initial position of the vehicle is assumed to be in (1,1). All 
of the constants and gains in the equations in previous sections are determined by trial 
and error methods, in order to be able to get a sufficient and desired results. 

As shown in Figure 3.5 below, when only the Long Range Navigation task is 
scheduled, the vehicle moves in a straight line, towards the target and reaches it without 


controlling its orientation. 
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Figure 3. 5 Long Range Navigation Trajectory. 


12 


When the vehicle is 5m away from the target, the Medium Range Navigation 
Phase starts, and a constraint on the vehicle orientation is introduced in the Lyapunov 


function. The trajectory of the vehicle including this phase can be seen in Figure 3.6. 


Long and Medium Range Navigation 
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Figure 3.6 Long and Medium Range Navigation Trajectory. 
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In Figure 3.6 we see the vehicle taking a bend to approach the target. When the 
vehicle is close to the target, since e goes to zero & and 6 change very quickly for small 
movements of the vehicle. | 

In our simulation, the vehicle begins the Short Range Navigation/Fine 
Maneuvering Phase, when it reaches the distance of 1 m. from the target. The resulting 


trajectory is shown in Figure 3.7. 
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Figure 3. 7 Long, Medium and Short Range Navigation Trajectory. 


The Long Range Navigation phase makes the vehicle head straight for the target. 
Its distance e from the target decreases and the angle & goes to zero. These are seen in 


Figure 3.8. 
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Figure 3. 8 Long Range Navigation Operational Variables ( e (t) and o (t) ). 


When executing the Medium Range Navigation phase, the vehicle approaches the 
target and lines up with the desired orientation, so minimizing the operational variables e, 


(a+). These variables can be seen in Figure 3.9. 
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Figure 3. 9 Medium Range Navigation Operational Variables ( e(t) and a(t)+d(t)). 
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In the fine maneuvering phase the distance e from the target goes to zero, and the 


orientation angle @ will do small oscillations but finally goes to the desired orientation. 


Short Range Navigation Phase 









3000 3100 3200 3300 3400 3500 3600 3700 3800 3900 4000 
time(sec) 





Baoo 3100 3200 3300 3400 3500 3600 3700 3800 3900 4000 


time(sec) 
Figure 3. 10 Fine Maneuvering Operational variables ( e(t) and $(t) ). 
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IV. SONAR BASED MOTION ESTIMATION AND CONTROL 
FOR AN AUV 
A. GENERAL 


In this chapter, a combined model of navigation, guidance and control in an AUV 
is addressed. In particular, a Lyapunov based control system for guidance and an acoustic 
motion estimation module are integrated in a two layered hierarchical architecture for 
closed-loop control [Ref. 8]. The guidance system is designed, as mentioned in Chapter 
I, on the basis of the definition of suitable Lyapunov functions for the different 
maneuvers in approaching a target. The motion estimation algorithm, as anticipated in 
Chapter II, is based on scanning sonar returns and uses the sonar range and bearing 
information in an Extended Kalman Filter-based structure. The overall estimation and 
control model is simulated in MATLAB SIMULINK model and the results are presented. 
B. THE ACOUSTIC MOTION ESTIMATION AND CONTROL MODEL 

As previously discussed, we are going to use a nested-loop Lyapunov based 
guidance algorithm of the integrated acoustic motion estimation model. A two layered 
hierarchical architecture for closed-loop control which does not require any planning has 
been designed through the definitions in Chapter II. This approach is suitable for 
handling the different target approach maneuvers, which require different precision in 
motion control, and so it introduces different phases of approaching the target based on 
the distance to it. On the other hand, this method includes an independent inner control 
loop for different tasks. 

For the motion estimation module in the integrated model, we are going to use the 
low speed AUV motion estimation model as previously discussed in Chapter IL This 
method allows high precision motion estimation in the direction orthogonal to a tracked 
linear reflecting surface for the case of structured environments. The vehicle's planar 
motion is estimated through the range and bearing measurements of a scanning sonar, 


using Kalman Filtering techniques. 
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Finally, a Kalman Filter based acoustic navigation module and a two nested loop 
guidance and control module have been combined to control the motion of an underwater 
vehicle. Chapter III dealt with the control algorithms based on Lyapunov techniques and 
the phases in the approaching a target for the vehicle. This will be our guidance and 
control module. On the other hand, the dynamic model of the Extended Kalman Filter 
based motion estimation technique from the scanning sonar measurements were discussed 


in Chapter II. This will be our motion estimation module for the combined model. 


Guidance/Control 


the estimated position the position/heading 


Sonar Range/Bearing 


Figure 4. 1 The Acoustic Motion Estimation and Control Model. 





In Figure 4.1, the ‘Guidance/Control' module represents the Lyapunov based 
control model, previously designed in Figure 3.2, and the ‘Estimation’ module represents 
the potential function based environment and motion estimation model, previously 
designed in Figure 2.3. 

C. SIMULATIONS AND RESULTS 


In this section we present the application of combined model of the vehicle 
motion and the potential function approach for the environment with two layered 
Lyapunov based control/guidance system. As an operating area we will assume a 
rectangular closed environment described by a suitable potential function. We are going 
to simulate the combined model with a MATLAB SIMULINK model shown in Figure 
4.2. The corresponding MATLAB files can be found in Appendix C. 
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Figure 4.2 The SIMULINK Model of the Motion Estimation and Control. 


Each simulation is organized into two phases: initialization and navigation 
between the initial point and the target. At the beginning, the vehicle builds a local map 
of the operating environment, which is a rectangular shaped pool with perpendicular 
walls. To do this, the vehicle stands virtually still (constant depth and orientation) and it 
scans the walls with its sonar. The orientation and the distance of each wall is computed 
using the 'slope estimation algorithm’, previously discussed in Chapter Il. The Three 
dimensional plot of the potential function is shown in Figure 4.3. 
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Figure 4.3 Potential Function of the Rectangular Pool Environment (24x24 m). 
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Upon completion of the initialization phase, the potential function model of the 
environment is constructed. Then, the vehicle moves between its initial position and the 
position of the target, estimating its motion on the basis of the algorithm presented in 
Chapter II, while updating the potential function model of the environment. The 
estimated positions are processed in the Control/Guidance module for each phase of the 
approach to the target. The Control/Guidance module computes the desired trajectory to 
be followed by the vehicle. A good performance of the algorithm relies on a consistent 
estimate of the vehicle's location by the estimator. 

For our case the vehicle is moving in a 24x24 m pool, starting at the origin (the 
(0,0) position) to a final target located at position (10,10). The desired orientation of the 
vehicle at the target is zero degrees in the target fixed frame. The contour plot of the 
potential function is shown in Figure 4.4, where we can see the location of the target at 
position (10,10). 
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Figure 4.4 The Contour Plot of the Potential Function Model of the Pool (24x24m). 
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Figure 4.5 Vehicle's Trajectory from Its Initial Position to the Target. 

The trajectory of the vehicle including all of the phases of approaching to the 
target, is shown in Figure 4.5. Firstly, the vehicle adjusts its heading to the target, then 
approaches it a distance of 7 m. on a straight line. When it is about 7 m. from the target, it 
maneuvers in order to be able to reach the target at the desired orientation. Finally, at the 
fine maneuvering phase, about 1 m. apart from the target, it moves to the target keeping 
its orientation. At the target, the resulting orientation is ¢=0 degrees and the vehicle is 


hovering above the target. 
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Figure 4.6 Vehicle's Orientation Angle During the Fine Maneuvering Phase. 
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V. SUMMARY, CONCLUSIONS AND RECOMMENDATIONS 


A. SUMMARY 


This study investigated the problem of navigation, guidance and control in 


Autonomous Underwater Vehicles. In particular, a Lyapunov function based guidance 


and control system and an acoustic motion estimation module are integrated in a two- 


layered closed-loop control architecture. The control system is designed on the basis of 


the definition of suitable Lyapunov functions for the different maneuvers in approaching 


a target. The motion estimation algorithm uses the range and bearing information of a 


scanning sonar in an Extended Kalman Filter based structure. The combined system is 


tested in computer simulations, and the results are presented in previous chapters. 


The following conclusions have been reached as a result of this study: 





B. CONCLUSIONS 
® 
@ 
@ 
& 


Estimation of vehicle position and velocity is possible with the motion 
estimation algorithm. 

The algorithm performed adequately through the actual sonar data gathered 
by the NPS Phoenix AUV. 

Lyapunov function approach and defining appropriate constants result in 
smooth control actions during vehicle motion for the control system 
algorithm. 

For the combined system, the Lyapunov based guidance system performed 
adequately in presence of uncertainty in position estimation. On the other hand 
the performance of the whole system relies on getting better estimates of the 
vehicle position and velocity. 

The algorithm is applicable to the NPS Phoenix AUV. The programs for the 
acoustic estimation module are written in C programming language and can be 


directly embedded to the original program of the AUV. 
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C. RECOMMENDATIONS 
The following recommendations are made for follow-on research in the on-going 
AUV development effort at NPS: 
e Convert the control algorithm programs which are written with MATLAB 
SIMULINK software package to the C programming language for direct 
implementation into the NPS Phoenix original program. 


e Generate sonar test data for scenarios of greater complexity to allow testing of 


the algorithm which goes beyond simulation/modeling. 











APPENDIX A. SONAR CHARACTERISTICS 


The primary sonar currently installed aboard the NPS Phoenix AUV is the Tritech 
ST1000 Profiling System. This profiling sonar is a high performance, high resolution 
compact and lightweight system which produces accurate underwater measurements. The 


following list provides a summary of the sonar performance characteristics and 


specifications: 
FLEQUENCY.......000csccccsccsesscsscsacensernene 1 MHz. 
BCA WIG isis siesdescnccawewidaaddenaaesemeanesaenen 0.9/1.8 degree conical 
Maximum Range...........scscccccccescrccseerssceseces 50 meters (160 feet) 
Minimum Range...........ccesceccesaccesscenseeseseens 0.3 meters (1 foot) 
WEIGDE eig.cehcecieeie si gueradnestsenemseaestne cee eeboeee 1.5 kg. 
VIEW SOCIOF iicassesessteeacsiiasegetenstesieeeawsiaes 360 degrees 
Scan Rate isco hase cans cides tise cuntimaneedectaataeseens 40 degrees/second 
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APPENDIX B. PROGRAM LISTINGS FOR SIMULATIONS 
IN CHAPTER II 


A. GENERAL 
This appendix contains the files generated for the simulations in Chapter II. They 

are written in C software language. The main program consists of C modules and their 
corresponding header files. These can be connected through a ‘Make file’ and run to get 
results. Basically, whole program reads data which is the real data collected from the 
scanning sonar of the AUV in the 6x6 feet rectangular test pool. The data length in the 
header file for initializing the variables, can be changed due to the length of the collected 
data. 
B. PROGRAM LISTINGS 
[AR EH A HH AE ACK He A AE A He A AE Oe He AE A He EE AE 8 EE EO 8 RR AC 2 OR 8 8 AO OE OE KE 

FILENAME: terms.h 

PURPOSE: ‘'Header' file to initiate all the program variables 

AUTHOR:  Hakki Celebioglu 

DATE: 1996 
OR AG AG Og 2 Ae OR Eg OC RE AR A HK AE A A A OO A AB A 2 KE OK EE A OE OK 2 OE A EE OK OK / 
#ifndef TERMS_H 
#define TERMS_H 
#define min(a,b) (((a)< (b)) ? (a):(b)) 
#define max(a,b) (((a)< (b)) ? (b):(a)) 
#define MULT3(a,b,c) (matrix_multiply(matrix_multiply(a,b),c)) 
#define ADDMULT2(a,b,c,d) (matrix_add(matrix_multiply(a,b),matrix_multiply(c,d))) 
#define OUT(a) (output_matrix(a)) 


int I; 
int J; /*cell indicators*/ 


double data[2168][2]; 


47 


/*enviroment parameters*/ 


double xmin; /*pool dimensions*/ 
double ymin; 

double xmax; 

double ymax; 

int Nx; /*number of the cells*/ 
int Ny; 

double dt; /*time interval*/ 
matrix neigbors_matrix; 
matrix] potential_matrixV; 
matrix] potential_matrix Vx; 
matrix] potential_matrix Vy; 
matrix! potential_matrixVp1; 
matrix! potential_matrix Vp2; 
matrix! potential_matrixVp3; 
matrix] point_1i; 

matrix] point_j; 

matrix covariance_matrix; 
matrix covariance_matrixP; 


matrix centroid_vector; 


/*kalman parameters*/ 
matrix A; 

matrix B; 

matrix acceleration_vector; 
matrix initial_covariance; 


matrix state; 
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int window_length; 

int IO; 

int JO; /*initial cells*/ 
matrix data_set; _ 


matrix posest; 


/*slope algorithm parameters*/ 
double eigenvalue; 


matrix eigenvector; 
#endif 


[RAR RAK ERA HR RR I RR EHR RR KH RRR KAR BRR ERB BRR ER KK KAA EAE 


FILENAME: matrix.h 
PURPOSE: ‘Header' file for matrix operators. In order to minimize the memory usage 
two different type is defined and used according to size of the operation. 
ofe ie ie he kc i aK oe 2 He ok 2 2 fe fe a oe ok 2 oe oe oo oo 2 2 2 2 fe 2k 2 2 2c 2k ok ok ok ok oo ok Ko OK OR oe KR RK KO OK EK KK KK A HK 
/ 
#ifndef MATRIX_H 
#define MATRIX_H 
/* defines the matrix data structure type */ 
typedef struct{ 
double m[30][30]; 
int row, col; 


}matrix; 


/* defines the large matrix data structure type */ 
typedef struct{ 
double m[50][50]; 
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int row, col; 
}matrixl; 
/* function prototypes for matrix.c */ 
matrix matrix_multiply(matrix mat1, matrix mat2), 
matrix matrix_add(matrix mat1, matrix mat2); 
matrix matrix _subtract(matrix mat1, matrix mat2); 
matrix matrix_transpose(matrix mat1); 
void output_matrix(matrix input_matrix); 
void output_matrix](matrixl input_matrix), 
matrix zeros_matrix(int N1,int N2); 
matrix] zeros_matrixl(int N1,int N2); 
matrix ones_matrix(int N1,int N2); 
matrixl ones_matrixl(int N1,int N2); 
matrix scalar_matrix(double C,matrix input_matrix); 
matrix! scalar_matrixl(double C,matrix] input_matrix); 
matrix matrix_diagonal(matrix mat]); 
matrix matrix_mean(matrix input_matrix); 
matrix identity_matrix(int N1); 
matrix matrix_col(int N1,matrix input_matrix); 


matrix matrix_row(int N1,matrix input_matrix); 


#endif 
[AACR IR ARR AA AAA R AR AAR AA TARAS A TR ATTA IAA AAA TAT IA IATA TE 
FILENAME: list.h 
PURPOSE: ‘Header’ file for including operational functions 
AUTHOR:  Hakki Celebioglu 
DATE: 1996 


fee HC OR HCO ACER RC RHC AC RC RC RAC ACER COR I RCE ER HC CRORE AE A ACR ER A AEE 


#ifndef LIST_H 
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#define LIST_H 
/* functions in list.c */ 
void read_data(int Length); 


void initial(double x_min,double y_min,double x_max,double y_max,int NX, int NY); 


void initial_kalman(int L,double initial_velx,double initial_vely); 
matrix initialize_data(int step,int L); 

matrix cartesian_conv(matrix input_matrix); 

void estimate(matrix input_matrix); 

int round(double number); 

void cell(matrix q); 

void eig(matrix mat1); 

void write_result(char filename[20],matrix mat1); 
void write_resultl(char filename[20],matrix! mat1); 
void potential(void); 

void slope(int length_data); 

#endif 


[RRA A RAHA AE RARE IR RK EH REA AERA KAA KK EHH RH ER HH EKA EHH EERE 


FILENAME: matrix.c 
PURPOSE: Create matrix operators to include addition, subtraction, 
multiplication, inverse and gauss_elimination, and 
create a rotation matrix, zero matrix, ones matrix, scalar matrix 
multiplication, diagonal matrix, mean of matrix colons, 
recover row and colons from matrix, identity matrix 
FUNCTIONS: matrix_multiplyQ 
matrix_add() 
matrix_subtract() 
matrix_transpose() 


output_matrix() 


51 











output_matrixl() 
zeros_matrix() 
zeros_matrixl() 
ones_matrix() 
ones_miatrixl() 
scalar_matrix() 
scalar_matrixlQ) 
matrix_diagonal() 
matrix_mean() 
matrix_row() 
matrix_col() 
identity_matrix() 
She A OR a aK oR 2 oR oR EEE AE HE 9 9 3 2G 2 KE 9 2 i i ok ik 2k 2k 2c 2 2c fc ofc fk 2 2 2 2k 2k 2k 9 0k 2 2k 2k ok 2k 2 2K OK OK OF OK OK OK OK OF 
*/ 
#include <stdio.h> 
#include <math.h> 
#include "matrix.h" 
[RRR KARA AME RR EB BB RA HH AK BBR HHH HH RR EE EE ER EE EE 
FUNCTION: matrix_multiplyQ 
PURPOSE: Miultiplies two matrix's together 
RETURNS: Matrix] * Matrix2 in a matrix data structure 


SH OK SCE EH RE HK A Eo EE HE KE A HE OK 2 i he 2 a fe 2 kG 2 ie 2G He 2 hk 2 2 2 2 2 I ig 2 2 Ik 2 OK og 2 OK OK OF EK 


af 

matrix matrix_multiply(matrix mat1, matrix mat2) 
{ 

int row, col, 1; 

matrix answer; 

/* conducts multiplication */ 


for (row=0; row<mat1.row; row++) { 
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for (col=0; col<mat?2.col; col++) { 


answer.m[row ][col]=0.0; 
for (i=0; i <matl1.col; i++){ 


answer.m[row][col] += mat1.m[{row]|[i] * mat2.m[i][col]; 


} 


/* assigns new row and col number to matrix data structure */ 
answer.row = mati .row; 
answer.col = mat2.col; 


return answer; 


} 


[RRA KAA AAA ERICH EHR RB REAR KAKA KK EAE BBA AR EAE RI EKER EKA EEK AER 


FUNCTION: matrix_addQ 

PURPOSE: Adds two matrix's together 

RETURNS: Matrix] + Matrix2 in a matrix data structure 
She HC AC HEH aK oe a a i 2K 2 2 KC a HE oR ie Ae 2 2 ke ig 2h 2 ok ie he 2h 26 2 ok 2k 2 2 2 a ie oe 2h 2 2 OO ie OE OK EO Ee KK OK 
*/ 
matrix matrix_add(matrix mat1, matrix mat2) 
{ 
int row, col; 
matrix answer; 
/* conducts addition */ 
for (row=0; row<matl.row; row++) { 

for (col=0; col<matl.col; col++) { 


answer.m[row]{col] = mat1.m[row][col] + mat2.m[row]|[col]; 


} 


/* assigns new row and col number to matrix data structure */ 
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answer.row = matl.row; 
answer.col = matl.col; 


return answer; 


} 


[RRR RAR AA AR AEE AH RR HK ERA RAE EB RAK RE BRR RE RR RE EERE EEE 


FUNCTION: matrix_subtract() 
PURPOSE:  Subtracts two matrix's from each other 
RETURNS: Matrix! - Matrix2 in a matrix data structure 


She he RK EC HO EK ESE IE KE a SE CHE OK HE KE EE he 2 2G i 9 fe 2c 3c 26 ooh 2 af 2c fe 2fe 0K 2c 2K 2c 2 2h 24s 2 2g 2 2h 26 ik 2 OK ofc 2 a 2 2K 2 2 2K OF 2 


se 
matrix matrix_subtract(matrix matl, matrix mat2) 
{ 
int row, col; 
matrix answer; 
/* conducts subtraction */ 
for (row=0; row<mat1.row; row++) { 
for (col=0; col<mat1.col; col++) { 


answer.m[row][col] = mat1.m[row]|[col] - mat2.m[row][col]; 


} 


/* assigns new row and col number to matrix data structure */ 
answer.row = matl.row; 
answer.col = mat1.col; 


return answer; 


} 


[RRR AK AR HRA RR I KR RK RK HK RRA ARK RAE KR HE ER RR ER EAE EE EE 


FUNCTION: matrix_transpose() 
PURPOSE: Creates the transpose of a matrix 
RETURNS: _ transpose(Matrix) in a matrix data structure 
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She Hee OE SICH HE A KEE REE i ak a a a IE a a af 2 a of i a 2 AC fe IE ae oe 2 ee 9 2 2 2c 2k 2c oe 2 2 2 2 2 AE EO Oe 3 oi 2 2G 2h ie ee EE EE 


*/ 

matrix matrix_transpose(matrix mat!) 

{ 

int row, col; 

matrix answer; 

/* conducts transpose */ 

for (row=0; row<matl.row; row++) { 
for (col=0; col<mat1.col; col++) { 


answer.m[col][{row] = mat1.m[row]|[col]; 


} 


/* assigns new row and col number to matrix data structure */ 
answer.row = matl.col; 
answer.col = matl.row; 


return answer, 


} 


[RAR HI RR KR RA AAR RIOR RR HOR IR KO AR HA A HAA RH RH RH RE HE HEAR ERE 


FUNCTION: output_matrixQ 
PURPOSE: Prints the contens of a matrix 
RETURNS: Void 
He OK HC CH HOKE Re HE GE EE RE A OE KE 2 AE AE 2 fe 2 2g 2 fe 2S he 2 fe 2 2 2 he 2 he 2 ie 2 2 2 2 AS 2 2k 24 2 EE 2 OE OK OE OK EE 
*/ 
void output_matrix(matrix input_matrix) 
{ 
int i,j; 
for (i=0;i<input_matrix.row;i++) { 
for(j=0;j<input_matrix.col;j++){ 


printf("%lf ",input_matrix.m[i]{j]); 
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} 

printf("\n"); 
} 
printf("\n"); 
} 


[RIOR AAACN ATOR AOA AAA AHA R RAR TAR AHH AA HR RH HATE BAHT RARE SE 


FUNCTION: output_matrixl() 
PURPOSE: Prints the contens of a matrix | 
RETURNS: Void 
ae HK OR ORE ESIC ICRC ER CIC IC EE 2 KC ER RR I EE 2 EE 28 2 AC Ee 2 A OE EE 2 2 2 Ee 2 2 EO EE OK 
cd | 
void output_matrixl(matrixl input_matrix) 
{ 
int 1,J; 
for (i=0;i<input_matrix.row;i++){ 
for(j=0;j<input_matrix.col;j++){ 
printf("%lf " input_matrix.m[i][j]); 
} 
printf("\n"); 
j 
printf("\n"); 
} 


[RRA AAT HAAR RR AIAN HATTA HH ARTE HAAR ERI TAT TEA TEBE SE 


FUNCTION: zeros_matrix() 

PURPOSE: Creates a matrix of zeros at given sizes 

RETURNS: zero(Matrix) in a matrix data structure 
eS RRR HCC COREE SF RRR 2K ECE OR CAC CE 2K 2K 2 EE EE 2 0 OE 2 CAE IE 2 EE 2 2 2 A I ee 2 OF EE OE OE OE 
*/ 


matrix zeros_matrix(int N1,int N2) 
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{ 


int row, col; 


matrix answer; 

answer.row=N 1; 

answer.col=N2; 

/* conducts zero */ 

for (row=0; row<N1; row++) for (col=0; col<N2; col++) 
answer.m[row][col] = 0.0; 


retum answer; 


} 


[HRA RAR HR RH RB BHR RAK AA KR RAE KBAR RE BE RR KE KE HE 


FUNCTION: zeros_matrixlQ) 
PURPOSE: Creates a matrix of zeros at given sizes 
RETURNS:  zero(Matrix)! in a matrix data structure 
oHe He HH KE KC A GE HE 2H KE ake oe ie 2 ie 2h 2 i 2A 2k oo oo 2 2 oe 2 2g 2k 2 ok ok oo 2 2 2 oe 2G 2G 2G ee 2k 2 2k 2 ok ok oo oe OK OK OK a i 2 EK OK OK OK 
il | 
matrix] zeros_matrixl(int N1,int N2) 
{ 
int row, col; 
matrix] answer; 
answer.row=N 1; 
answer.col=N2; 
/* conducts zero */ 
for (row=0; row<N1; row++) for (col=0; col<N2; col++) 
answer.m [row ][col] = 0.0; 


return answer; 


j 


[HAA RAHA AR RR BR BH IE A A A A AH A AAR ROR EE REE EEE EE EE 


FUNCTION: ones_matrix() 


oy. 








PURPOSE: Creates a matrix of ones at given sizes 
RETURNS:  ones(Matrix) in a matrix data structure 

fe HH HERR OR HRC ACE A EA HE AC EE ER HE AC 2 AE 9 2 2 2 fe 2 2 HE IE EE 9 9 2 ae AE A 2 AC 2 EEE OK 

wl | 

matrix ones_matrix(int N1,int N2) 

{ 

int row, col; 

matrix answer; 

answer.row=N 1; 

answer.col=N2; 

/* conducts ones */ 

for (row=0; row<N1; row++) for (col=0; col<N2; col++) 

answer.m[row]|[col] = 1.0; 


return answer, 


} 


[RAHA HORA A AAR ARR ARTA RHEK AAR ATT THR BAER HH EE EEE EES 


FUNCTION: ones_matrixlQ 
PURPOSE: Creates a matrix of ones at given sizes 
RETURNS: ones(Matrix)] in a matrix data structure 
shee HER A CH HERO KOK KE A EE ACE RIE EE A AC A EE EE EE IC CK 2 2 He 2 2 2 Fe IE 2 2 Ae 2A 2 a eC IE 9 KK 2K 
*/ 
matrix] ones_matrixl(int N1,int N2) 
{ 
int row, col; 
matrix] answer; 
answer.row=N 1; 
answer.col=N2; 


/* conducts ones */ 
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for (row=0; row<N1; row++) for (col=0; col<N2; col++) 


answer.m[row]|[col] = 1.0; 
return answer; 


} 


[RAR ARIA HAH AHA RR HRI RR HRA HR HR HAHA AA A AHH AIR HRA RHR EER ER EE EEE 


FUNCTION: matrix scalar_matrix() 
PURPOSE: multiplies a scalar with matrix 
RETURNS: | result(Matrix) in a matrix data structure 
eK HEHE HC RR RR I EK oR CE OK EER EE EE EE EE EE EE 2 fe A Ee A IE Ee EE KK OK 
al f 
matrix scalar_matrix(double C,matrix input_matrix) 
{ 
int row, col; 
matrix answer; 
/* conducts multiplication */ 
for (row=0; row<input_matrix.row; row++) { 
for (col=0; col<input_matrix.col; col++) { 
answer.m|[row][col] = C*input_matrix.m[row]|[col]; 
} 
j 


answer.row=input_matrix.row; 
answer.col=input_matrix.col; 


return answer; 


j 


[RAR AAR AHR AHR RAR HHH RHR KR EEA AREA BARAK HOR HERR BERR AK AEE EA EEE 


FUNCTION: matrix scalar_matrixlQ 
PURPOSE: multiplies a scalar with matrix 
RETURNS: | result(Matrix)1 in a matrix data structure 
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We He HCHO I HOR ROR RIS HOR AACR SR ACO RCA OE CRC EER EO ACO HC EE EB 2 ER A A AE HE AE 


3 | 
matrix] scalar_matrixl(double C,matrixl input_matrix) 
{ 
int row, col; 
matrix! answer; 
/* conducts multiplication */ 
for (row=0; row<input_matrix.row; row++) { 
for (col=0; col<input_matrix.col; col++) { 


answer.m[row]|[col] = C*input_matrix.m[row][col]; 


} 
} 


answer.row=input_matrix.row; 
answer.col=input_matrix.col; 


return answer; 


} 


[RRA AAR AA AAR TARA AAR AAR RAR HARRIET AAA AT HHI ITA III TEE 


FUNCTION: matrix_diagonal() 
PURPOSE: writes elements of one dimensional matrix to diagonal 
RETURNS: Diagonal(Matrix) in a matrix data structure 
We HH ARR RHR HOR HOR RR RRR IC IEEE EE EEE IEE EE EE EE IE EE IEE A EEE AEE EE 2 EE EE EE 
| 
matrix matrix_diagonal(matrix mat1) 
{ 
int row, col; 
matrix ans; 
/* conducts diagonal */ 
if (matl.row==1 || mati.col==1){ 


if (matl.row!=1) 
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matl=matrix_transpose(mat1); 


for (col=0; col<matl.col; col++) 
ans.m[col][col] = mat1.m[0][col]; 
ans.row = mat1.col; 

ans.col = matl.col; 


} 


for(row=0;row<ans.row;row++)for(col=0;col<ans.col;col++) 

{ 

if(row!=col) 

ans.m[row][col]=0.0; 

} 
return ans; 
, 
[ERA AK KKK RAHI RRA RIK HR EH KH KH KHER BRE HH BH ERE ER ERE EEE EE 

FUNCTION: matrix_mean() 

PURPOSE: Gets mean of the colons of the matrix 

RETURNS: mean (matrix) in a matrix data structure 
fe OE EK HK ACH A RE EE KE a HE OE He 2 2G Ae Ae He 2 2K i 2 fe 2 fe 2 iE i 2G ig 2 ie 2 he 2 fe 2 2 oe 2g og 2 ie 2 he 2 he 2 2 OK 2 OF 
sy 
matrix matrix_mean(matrix input_matrix) 
{ 
int row,col; 
double a=0.0; 
matrix answer; 
/* getting mean of colons */ 

for (col=0; col<input_matrix.col; col++){ 

for(row=0; row<input_matrix.row;row++){ 
a+= input_matrix.m[row]|[col]/(double) input_matrix.row ; 


answer.m[O][col]=a; 
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j 
=0.0; 
} 
answer.row=1; 
answer.col=input_matrix.col; 


return answer; 


} 


[RRA AAA RATT RA AKA A REAR A HAART EA AA THAT AR BRI HHH TATA AA RES 


FUNCTION: matrix_row() 
PURPOSE: Gets given rows of the matrix 
RETURNS: _result(in given sizes) in a matrix data structure 


She HRN EO HEC OR ER ESI CE CAR AE 2 ER AE I AE C2 EE 2 2 a OE 2 CE EE 2 ee 2 2 2 FE 2 2 2 EE EE 


=} 

matrix matrix_row(int N1,matrix input_matrix) 

{ 

int col; 

matrix answer; 

/* getting wanted row */ 

for (col=0; col<input_matrix.col; col++) { 

answer.m{[O][col] = input_matrix.m[N1-1]{col]; 
j 

answer.row=1; 

answer.col=input_matrix.col; 


return answer; 


} 


[RRA OR IC OR RR ER A OR ACR AR ACE ER TTR A AA HE A HR AT AHIR TR EH EE REE EES 


FUNCTION: matrix_colQ 
PURPOSE: Gets given col of the matrix 
RETURNS: _ result(one col) in a matrix data structure 
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HoH KE KC RC OR HG HO KE ae 2G KE a EE 2 KC a 2 AE 2 2K a he 2A 2 2 oc He fe 2 EO Ae AE 2G ok 2 2 2 2 2 2 2 2 2 ik 2 2k fe 2 2 OK oe 2s ie oe 2 of OK OK 2 2 eK 


*/ 

matrix matrix_col(int N1,matrix input_matrix) 

{ 

matrix answer; 

/* getting wanted col */ 
answer=matrix_transpose(input_matrix); 
answer=matrix_transpose(matrix_row(N 1,answer)); 
returm answer; 


} 


[RRR RAR AAA AH A A I HO EK A HR RHE KK BR RB HH A A ER A A EK HE EE 


FUNCTION: identity_matrix() 
PURPOSE: _ establish given size identity matrix 
RETURNS: identity matrix in Matrix structure 
ofe eK KC KC ai fe 2 I 2 ic fe Kt oc ic oe 2 oe i i oo 2 2 2 2 2 2 oe 2K 2k oie 2 oe 2 2k fe fe 2 eK 2 he ie fe oO OO OK Oo OR OK Oe i ee EK KOK 
st 
matrix identity_matrix(int N1) 
{ 
matrix answer; 
answer=ones_matrix(1,N1); 
answer=matrix_diagonal(answer); 
return answer; 


} 
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[RR RR ARACEAE HER RR RR A AR A HA HEIR EBA A AAA AE REE EES 


FILENAME: list.c 
PURPOSE: All operations of the algorithm 
AUTHOR: Hakki Celebioglu 
DATE: 1996 
She HAH RHEE HOR I HR HERE aE a 2K 2 AC EE AEE EE 2 I AE IE 2 AE EE EE 2 2 IE 2 AE AS 2 EO CHE 2 EOE / 
#include <stdio.h> 
#include <math.h> 
#include "matrix.h" 
#include "terms.h" 
#include "list.h" 
#define pi 3.14159265358979 
[* 

Functions 
read_data() 
initial() 
initial_kalman() 
initialize_data() 
cartesian_conv() 
estimate() 
round() 
cellO 
eig 
write_result() 
write_resultlQ 
potential() 
slope() 


[RRA ARATE IR AAR ARR RRA AA HR EAA AA AA HB A ATA HHA AA EEE 


FUNCTION: read_data() 














PURPOSE: Reading file to an array defined globally 
RETURNS: void 


ORE 2 AG oe ee 2 ie 2 fe 2 2 ae 2 2 fe 2 fe ok 2H ok 2 fe oe 2 he 2 fe 2 fe 2 2c a 2 he 2 Ae 2 ie 2 ie 2 Ae ie 2 ie 2 ee 2 ie 2 fe 2 Ae 2 2 2 2K 2 OE 


void read_data(int Length) 


{ 
int row,col; 
FILE *fp; 
if((fp=fopen("test.txt","r"))==NULL) 
{ 
fprintf(stderr,"Error openning"); 
} 


for(row=0;row<Length;row++)for(col=0;col<2;col++){ 
fscanf(fp,"%lf ",&data[row][col]); 


} 
fclose(fp); 


} 


[ER AE AE AE A ER Ee AE KK OR AE HE A A A A A EE 8 EO EE HE ER EE 


FUNCTION: initialQ 
PURPOSE: _initilize neighbors_matrix,I,J,10,JO,potential matrices 
point_i,point_j,xmin,xmax,ymin,ymax,Nx,Ny 
RETURNS: void (all are globally decleared) 
oe oe fe 2 fe 246 26 Ke fe 2 2c 2 a 2 2 fe 2A a Ae 2 fe 2 ot he Fe 2 AE OK he 2 ik fe 2 2s 2 fe 2A 2 A OK he EO eo 2 OR 2 a He OK ee 2 he 2 ie 246 2 ae 2 OE 2 2 OK 
void initial(double x_min,double y_min,double x_max,double y_max,int NX,int NY) 
{ 
double a,b,c; 
a=sqrt(2.0); 
b=2.0*a; 
c=sqrt(5.0); 


xmin=x_min; 
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ymin=y_min,; 

xmax=x_max; 

ymax=y_max; 

Nx=NX; 

Ny=NY; 

dt=0.1; 

potential_matrix V=zeros_matrixl(Nx,Ny); 
potential_matrix Vx=potential_matrixV; 
potential_matrix Vy=potential_matrixV; 
potential_matrix Vp1=potential_matrixV; 
potential_matrix Vp2=potential_matrixV; 
potential_matrix Vp3=potential_matrixV; 
potential_matrix V=scalar_matrixl( 10.0,ones_matrixl(NX,NY)); 
point_i=scalar_matrixl(-1 .0,ones_matrixl(NX,NY)); 
point_j=point_i; 
neigbors_matrix.m[0][0]=b; 
neigbors_matrix.m[0][1]=c; 
neigbors_matrix.m[0][2]=2.0; 
neigbors_matrix.m[0}[3]=c; 
neigbors_matrix.m[0][4]=b; 
neigbors_matrix.m[1][0]=c; 
neigbors_matrix.m[1]{1]=a; 
neigbors_matrix.m[1][2]=1.0; 
neigbors_matrix.m[1][3]=a; 
neigbors_matrix.m[1][4]=c; 
neigbors_matrix.m[2][0]=2.0; 
neigbors_matrix.m[2][1]=1.0; 
neigbors_matrix.m[2][2]=0.0; 
neigbors_matrix.m[2][3]=1.0; 
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neigbors_matrix.m[2][4]=2.0; 


neigbors_matrix.m[3][O]=c; 
neigbors_matrix.m[3][1]=a; 
neigbors_matrix.m[3][2]}=1.0; 
neigbors_matrix.m[3][3]=a; 
neigbors_matrix.m[3][4]=c; 
neigbors_matrix.m[4][0]=b; 
neigbors_matrix.m[4][1]=c; 
neigbors_matrix.m[4][2]=2.0; 
neigbors_matrix.m[4][3]=c; 
neigbors_matrix.m[4]}[4]=b; 
neigbors_matrix.row=5; 
neigbors_matrix.col=5; 


} 


[9H HA AH A AR RB IIE A BR BR A HC A A A A A AA A AR EE OE EE EE EE EE HE 


FUNCTION: initial_kalman() | 

PURPOSE: _ initilize Kalman dynamics(initial_state,initial_covariance, 
window_length,A,B matrices 

RETURNS: void ( all are globally decleared) 


SHE Ke OK i 2 KE Ke HG KE Ae KE AK 2h OK he 2 he 2 he oe Ke HE OK OK A fe 2 2 he 2 a Fe OE 2 iG 2 ig 2 a 2 iS IC I i OE EO Re 2 ie OK he OK EK EK 


void initial_kalman(int L,double initial_velx,double initial_vely) 


JO=0; 
A=zeros_matrix(4,4); 
A.m{0][2]=1.0; 
A.m{1][3]=1.0; 
A.row=4; 


A.col=4; 
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A=matrix_add(identity_matrix(4),scalar_matrix(dt,A)); 
B.m[0][0}]=0.0; 

B.m[0][1]=0.0; 

B.m{1][0]=0.0; 

B.m{[1]{1]=0.0; 

B.m[2][0J=1.0; 

B.m[2][1]=0.0; 

B.m[3][0]=0.0; 

B.m[3][1]=1.0; 

B.row=4; 

B.col=2; 

B=scalar_matrix(dt,B); 
state.m[0][0}=0.0; 
state.m[1][0]=0.0; 
state.m[2][0]=initial_velx; 
state.m[3][0]=initial_vely; 
state.row=4; 

state.col=1; 
initial_covariance.m[0][0]=0.0; 
initial covariance.m[0][1]=0.0; 
initial covariance.m[0][2]=0.01; 
initial_ covariance.m[0][3]=0.01; 
initial_covariance.row=1; 

initial _covariance.col=4; 

initial. covariance=matrix_diagonal(initial_covariance); 
window_length=L; 
acceleration_vector.m[0][0]=0.0; 
acceleration_vector.m[1][0]=0.0; 


acceleration_vector.row=2; 
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acceleration_vector.col=1; 


} 


[BRIAR A AAR AA AHH RIOR IR IR OR IR RK AH AAA HE EHH RH EHR RA EAE 


FUNCTION: initialize_data() 
PURPOSE: Reading file to a matrix (size*2(angle&range) windowing) 
RETURNS: data matrix in Matrix structure 
HOOK HK HC KC Ko a he He oH oe ie He KE a HE OK ie fe 2 aK fe 2h 2 2 a fe fe 2 2 og 2 2 OK ee 2 2 OK 2 OK Oe 2 2 OK eo KK OK OK OB OK OK RO OE 
*/ 
matrix initialize_data(int step,int L) 
{ 
int row,1; 
matrix answer]; 
int size=L*2+1; 
i=0; 
for(row=step* size;row<step*size+size;row++) { 
answer 1.mf[i][O]=data[row][0]; 
answer! .m[i][1J=data[row][1]; 
I++; 
j 
answer 1 .row=Ssize; 
answer 1 .col=2; 


return answerl1; 


} 


[RRA KARA A AA ARR HHH KARR KR AH A HK HR RAH A AR BE I KR BK RE ERE EEE 
FUNCTION: cartesian_conv() 


PURPOSE: making conversian (theta&rho) to (x&y) 
RETURNS: matrix in Matrix structure 


RHEE KG A HE OK 2K OK i 2 2 2G OE A A 2 2 OK 2 2G 2 2 OK ig IK IC 9 IE 2G 2S 2 2 2 9 2 2c 2h 296 2 2 ok 2h 2g 2 2 2 9 ig Ae Ae fe 2 he 2 2 oe oe 2 OF 3 KE Ok 


*/ 
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matrix cartesian_conv(matrix input_matrix) 
{ 
int row,col; 
matrix answer,answerl; 
matrix sl,s2,theta,rho; 
double a=pi/180.0; 
theta=matrix_transpose(scalar_matrix(a,matrix_col(1,input_matrix))); 
for(row=0;row<theta.row;row++) for(col=0;col<theta.col;col++){ 
s1.m[row][col]=cos(theta.m[row]|[col]); 
s2.m[row][col]=sin(theta.m[row][col]); } 
sl.row=theta.row; 
s2.row=theta.row; 
sl.col=theta.col; 
s2.col=s1.col; 
for(col=0;col<theta.col;col++){ 
answer1.m[0][col]=s1.m[O][col]; 
answer 1.m[1][colJ=s2.m[0][col]; 
} 
answer] .row=2*theta.row; 
answerl.col=theta.col; 
rho=matrix_diagonal(matrix_col(2,input_matrix)); 
answer=matrix_multiply(answer1,rho); 
answer.row=input_matrix.col; 
answer.col=input_matrix.row; 


return answer; 


} 


[RHA ARIA RRA HRA AH AAA ARTE RAK EE AE HR HEAR AE HAHA EER EEE EEE 


FUNCTION: estimate() 


PURPOSE: estimates covariance matrix and centroid vector 
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RETURNS: void (parameter would be defined global 
so these will be initialize in a matrix data structure(P and c) 
P~covariance_matrix, c~centroid_vector(one colon) 
oe fe oe fe oe ic ake oie fe 2 2K ie 2 ik oie 2k oe oie fe 2 oe 2 oe 2c ke fe oe 2k oe 2 ik 2 2k oe oie fe 2 2k 9 2h oe 2h 2 2 fe oi 2k ik 2 2 2g 2 ie 2 2 oR 2k ko 2k oe 2 oe OK OE OK OK OK 
*/ 
void estimate(matrix input_matrix) 
{ 
matrix Q,QQ,PP; 
centroid_vector=matrix_transpose(matrix_mean(matrix_transpose(input_matrix))); 
QQ=matrix_multiply(centroid_vector,ones_matrix(1,input_matrix.col)); 
Q=matrix_subtract(input_matrix,QQ); 
PP=matrix_multiply(Q,matrix_transpose(Q)); 
covariance_matrix=scalar_matrix(1.0/(double)input_matrix.col,PP); 


} 


[RAR RR A RR A KH AK A A HK AR HH AK HH AE A HR A EE RR EB EE EE EE 


FUNCTION: round() 
PURPOSE: rounds the number 
RETURNS: integer rounding 
oie 2 ofc 246 fe ik 2k ok of ofc ie oie ie oe 2 oe 2 2k oe 2 OK oie oe 2 2 2 oe Ke oie fe fe oye oe fk oie ie fe oe 2 OK 2 2 oe ie 2c oR 2k OK kg 2 2K 2 oe 2 2 oS 2 so OO 2 OB RB Ok OK 
sa | 
int round(double number) 
{ 
int no; 
if (number>0) 
{ 
if(number+0.5>ceil(number)) 
number = ceil(number); 
else 


number=floor(number); 
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no=(int)number; 


} 


else 


{ 


number=-1.0*number; 
if(number+0.5>ceil(number)) 
number = ceil(number); 

else 

number=floor(number); 


no=(int)number; 


no=-1*no; 
return no; 


} 


[RRR AHA ARIA ARIA HR RR ATA ER AR HHA AAA HR AH HE AT A AER EEE 


FUNCTION: cellQ 
PURPOSE: calculates the values of cell indicators LJ 


input would be a vector 


RETURNS: void ( I&J are globally decleared) 


[RRR A AAA AAR AAR ARATE AAA A RATT RE ARTA EAA A TAT RE ATA TIARA EEE 


/ 
void cell(matrix q) 
{ 
double qq,qqq,deltax,deltay; 
int V,VV; 
deltax=(xmax-xmin)/(double)Nx; 
deltay=(ymax-ymin)/(double)Ny; 
if (q.row>q.col){ 
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qq=(q.m[0][0]-xmin)/deltax; 


v=round(qq); 
qqq=(q.m[1][0]-ymin)/deltay; 
vv=round(qqq); 

if(v<1)v=1; 

if(vv<1)vv=1; 

if(v>Nx) v=Nx; 

if(vv>Ny) vv=Ny; 

I=v; 

J=vv; 

} 

else{ 
qq=(q.m[0][0]-xmin)/deltax; 
v=round(qq); 
qqq=(q.m[0][1]-ymin)/deltay; 
vv=round(qqq); 

if(v<1)v=1; 

if(vv<1)vv=1; 

if(v>Nx) v=Nx; 

if(vv>Ny) vv=Ny; 

T=v; 

J=vv; 


} 





[HRA AAA ARR RR RR RR BRR KK HAA AH HB BBR HE EE AE A ETE HE HE HE 


FUNCTION: eig() 


PURPOSE: computes eigenvalue and eigenvector of 2*2 matrix 


RETURNS:  void(globally) 
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se eRe RCH HE RHE SR RR CHECK CE ECG 2 EEK EE EH IC HE EE OK EK AC He OB EE EE EK HE 


7} 
void eig (matrix mat!) 
{ 
double tol,],Isav,kk; 
matrix X,Z; 
if(matl.m[O][(0J==0.0 &&  matlm[O][1]==00 && matl.m[1][OJ==0.0 && 
mat1.m[1][1]==0.0) 
{ 
eigenvalue=0.0; 
eigenvector.m[0][0]=0.0; 
eigenvector.m[1][0]=1.0; 
eigenvector.row=2; 
eigenvector.col=1; 


} 


else 
{ 
1=0.0; 
Isav=1.0; 
tol=0.00001; 
x=ones_matrix(2,1); 
while(fabs(Isav-1)> tol) 
{ 
Isav=l; 
z=matrix_multiply(mat1,x); 
l=max(fabs(z.m[0][0)),fabs(z.m[1][0])): 
x=scalar_matrix((1.0/1),z); 
} 


eigenvalue=|; 
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eigenvector=x; 

kk=eigenvector.m[0][0]/eigenvector.m[1][0]; 
eigenvector.m[1][0]=sqrt(1.0/(1+kk*kk)); 

el genvector.m|[0] [O]=sqrt(1-eigenvector.m[1][0]*eigenvector.m[1][0]); 
if (kk<0) 

eigenvector.m[0][0]=-1.0*eigenvector.m[0][0]; 

eigenvector.row=2; 


eigenvector.col=1; 


} 
} 


[FEAR A RE HE EK EK KK KR BA CR A EB HE OE RA EE HE EE EK EE 


FUNCTION: potentialQ 
PURPOSE: Estimates the potential function of the enviroment(cells) 
RETURNS: fills all the globally decleared variables 
2K of ofc ofc ofc 2 2c 2 3k 2 2 i fe 2k 24 IE OK 2 9 Kk 2 2 ok 2 of 2c i 2 2k 2 2 oe fe 2 ok oie fe 2 2 oe Re 2 ok oo OK 2 OR ie i 2 2k Oe oe ok OK Ok OK OK OK KK OK KK 
*/ 
void potential(void) 
{ 
int iter,1,tt,ttt,11,]},1S,jS,ns; 
matrix s,sL,tip_sonar,tempq,temp; 
double D1,D2; 
for (iter=2*window_length+1;iter<=400;iter++) 
{ 
s=cartesian_conv(initialize_data(iter-1,0)); 
state=matrix_add(matrix_multiply(A,state),matrix_multiply(B,acceleration_vector)); 
if (data_set.row>data_set.col) 
ns=data_set.row; 
else 


ns=data_set.col; 
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for(ttt=0;ttt<data_set.row;ttt++) 
{ 
sL.m{ttt][0]=data_set.m[ttt][ns-window_length-1]; 
sL.row=data_set.row; 
sL.col=1; 
} 
tempq.m[0][0]=state.m[0][0]; 
tempq.m[1][0]=state.m[1][0]; 
tempg.row=sL.row; 
tempq.col=sL.col; 
tip_sonar=matrix_add(tempq,sL); 
cell(tip_sonar); 
/*printf("%d\t%d\tFod\t%ad\n" ,L,J,10,J0); st 
if I==10 && J==JO) 
{ 
data_set.col=data_set.col+s.col; 
data_set.m[0][data_set.col-1]=s.m[0][0]; 
data_set.m{1][data_set.col-1]=s.m[1][0]; 
posest.col=posest.col+s.col; 
posest.m[0][posest.col-1]=state.m[0][0]; 
posest.m[1][posest.col-1]=state.m[1][0]; 


} 


else 
{ 
estimate(matrix_add(posest,data_set)); 
covariance_matrixP=covariance_matrix; 
=0; 
for(i=(ns-2*window_length)-1;i<ns;i++) 


{ 
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tempq.m[0][tt]=data_set.m[0][1]; 
tempq.m[1][tt]=data_set.m[1][1]; 
temp.m[0][tt]=posest.m[0][1]; 
temp.m[1][tt]=posest.m[1) fi]; 
tt++; 
} 
data_set=tempq; 
posest=temp; 
data_set.row=2; 
data_set.col=2*window_length+1; 
posest.row=2; 
posest.col=2*window_length+1; 
IO=I; 
JO=J; 
if 0!=0 && JO!=0) 
{ 
potential_matrix Vx.m[I0-1][JO-1]=centroid_vector.m[0J[0]; 
potential_matrix Vy.m[I0-1][JO-1]=centroid_vector.m[1][0]; 
potential_matrix Vp1.m[I0-1][JO-1]=covariance_matrixP.m[0][0]; 
potential_matrix Vp2.m[I0- 1][JO-1]=covariance_matrixP.m[1][1]; 
potential_matrix Vp3.m[I0- 1][JO-1]=covariance_matrixP.m[0][1]; 
potential_matrix V.m[I0-1][JO-1]=0.0; 
for (ii=0;i1<5;ii++) { for(jj=0;jj<5;jj++) 
{ 
iS=1i-2; 
js=Jj-2; 
if(10+is>0&&IJ0+js>O&&IO+is<Nx+1&&JO<Ny+1) 


{ 
D1=potential_matrix V.m[I0+is-1][JO+js-1]; 


7] 








D2=neigbors_matrix.m[ii}[jj]; 
if(D2<D1) 
{ 
potential_matrix V.m[I0+is-1][J0+js-1]=D2; 
point_i.m[I0+is-1][JO+js-1]=(double)I0; 
point_j.m[I0+is-1][JO+js-1]=(double)JO; 
} 
} 
}} 
point_i.m[IO-1][JO-1]}=-1.0; 
point_j.m[IO-1][JO-1]=-1.0; 
j 


j 


[RRA AR HOR IRE RH A AR HE RE HR RE ER A ER HR A A ER AEE HE EEE 


FUNCTION: slope() 
PURPOSE: Estimates the slope of the enviroment 
RETURNS: _ filling the the slope variables 
SA SR SR SR ACR AC ACR ER ER EE CEA HE A OER ER EF EO EA ACR ER A EH A EEE EE 
| 
void slope(int length_data) 
{ 
int iter,tt,ttt,i,slope_window,11,j1; 
matrix s,tempg,covariance_matrix W temp; 
matrix eigenvectorw,eigenvectorp,phi,H,K; 
matrix! Q; 
double eigenvaluep,indice,R,den; 


FILE *fp; 
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re 


if((fp=fopen("result1.m","w"))==NULL) 
fprintf(stderr,"Error openning"); 
fprintf(fp,"xm=["); 


slope_window=10; 
state=matrix_add(matrix_multiply(A,state),matrix_multiply(B,acceleration_vector)); 
for (tt=401 ;tt<=400+slope_window;tt++) 
{ 
=cartesian_conv(initialize_data(tt-1,0)); 
if(tt==401) 
{ 
data_set=s; 
posest.m[0][0]=state.m[0][0]; 
posest.m[1][O]=state.m[1][0]; 
posest.row=2; 
posest.col=1; 


} 


else 
{ 
data_set.col=data_set.col+1; 
data_set.m[0][data_set.col-1]=s.m[0][0]; 
data_set.m[1][{data_set.col-1]=s.m[1][0]; 
posest.col=posest.col+1; 
posest.m[0][posest.col-1]=state.m[0][0]; 
posest.m[1][posest.col-1}=state.m[1][0]; 


j 
fprintf(fp," Zlf\t lft Zlf\t%lfin" ,state.m[0][0],state.m[1][0],state.m[2][0],state.m[3][0}); 


} 
Q=zeros_matrixl(Nx,Ny); 


79 





for (iter=401+slope_window;iter<=length_data;iter++) 
{ 
estimate(matrix_add(posest,data_set)); 
covariance_matrixW=covariance_matrix; 
cell(centroid_vector); 
while ((point_i.m[I-1][J-1]!=-1.0) && (point_j.m[I-1][J-1]!=-1.0)) 
{ 
il=(int) point_i.m[I-1][J-1]; 
jl=(int) point_j.m[I-1][J-1]; 
=11; 
J=jl; 
} 
Q.m[I-1][J-1}=1.0; 
centroid_vector.m[0][0]=potential_matrix Vx.m{[I-1][J-1]; 
centroid_vector.m[1][0]=potential_matrix Vy.m[I-1][J-1]; 
covariance_matrixP.m[0][0]=potential_matrix Vp1.mf[I-1][J-1]; 
covariance_matrixP.m[0][1]=potential_matrix Vp3.m[I-1][J-1]; 
covariance_matrixP.m[1][0]=potential_matrix Vp3.m{I-1][J-1]; 
covariance_matrixP.m[1][1]=potential_matrix Vp2.m[I-1][J-1]; 
covariance_matrixP.row=2; 
covariance_matrixP.col=2; 
eig(covariance_matrixP); 
eigenvectorp=eigenvector; 
eigenvaluep=eigenvalue; 
eig(covariance_matrixW); 
eigenvectorw=eigenvector; 
if (eigenvaluep!=0.0) 
{ 
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re 


tempg=MULT3(matrix_transpose(eigenvectorw) ,covariance_matrixP,eigenvectorw); 
indice=tempq.m[0][0]/eigenvaluep; 
} 
R=0.1; 
if (indice < 0.9 Il covariance_matrixP.m[0] [0]+covariance_matrixP.m[1][{1]==0.0) 
=100000.0; 

phi.m[0][0]=eigenvectorp.m[1][0]; 

phi.m[1][0]=-1.0*eigenvectorp.m[0][0]; 

phi.row=2; 

phi.col=1; 

H.m[0][0]=phi.m[0}][9]; 

H.m[0][1]=phi.m[1][0]; 

H.m[0][2]=0.0; 

H.m[0][3]=0.0; 

H.row=1; 

H.col=4; 

tempq=MULT3(H. initial_covariance,matrix_transpose(H)); 

den=R+tempq.m[0)[0]; 

K=scalar_matrix(1.0/den,MULT3(A, initial_covariance,matrix_transpose(H))); 

temp=matrix_subtract(centroid_vector,s); 

temp=matrix_multiply(matrix_transpose(temp),phi); 

temp=matrix_subtract(temp,matrix_multiply(H,state)); 
state=matrix_add(ADDMULT2(A;state,B,acceleration_vector),matrix_multiply(K,temp)) 
initial_ covariance=matrix_subtract(MULT3(A, initial_covariance,matrix_transpose(A)),m 
atrix_multiply(scalar_matrix(den,K),matrix_transpose(K))); 

=cartesian_conv(initialize_data(iter-1,0)); 


for(ttt=0;ttt<data_set.row;ttt++) for G=2;i<=slope_window;i++) 
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{ 


data_set.m[ttt][i-2]=data_set.m[ttt][i-1]; 
posest.m[ttt][i-2]=posest.m{[ttt}[i-1]; 
| 
data_set.row=data_set.row; 
data_set.col=slope_window- 1; 
posest.col=slope_window-1; 
data_set.col=data_set.col+1; 
data_set.m[0][data_set.col-1]=s.m[0][0]; 
data_set.m[1][data_set.col-1]=s.m[1][0]; 
posest.col=posest.col+1; 
posest.m[0] [posest.col-1]=state.m[0][0]; 
posest.m[1 ][posest.col-1]=state.m[1][0]; 
fprintf(fp," Mlf\tZlft%If\tZlfin" ,state.m[O] [0],state.m[1][0],state.m[2][0],state.m[3][0]); 
} 
fprintf(fp,"];figure(1),plot(xm(:,1));figure(2),plot(xm(:,2));figure(3),plot(xm(:,3));figure(4 
) plot(xm(:,4))3"); 
j 


[RIOR ARRAN TAR A HAAR HATA TAT AHH EAA HH HT BE ITAA BHAA EES 


FUNCTION: write_result() 
PURPOSE: Writing a file 
RETURNS: creates a file for pltting 
ae ek ke RR HHH CRC RE SR ROR RCI HC HCE 2 AC EE EE 2 2 ECE EE 2 FACE HE oo e229 OO EK 
+) 
void write_result(char filename[20],matrix mat1) 
{ 
int row,col; 
FILE *fp; 
if((fp=fopen(filename,"w"))==NULL) 
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fprintf(stderr,"Error openning"); 


for(row=0;row<mat1.row;row++) 
{ 
for(col=0;col<mat1.col;col++) 
fprintf(fp,"%lf\t ",mat1.m[row]{col]); 
fprintf(fp,"\n"); 
} 
fclose(fp); 
} 


[RRA RAR AKA A AR RR RHR RAK A KK KAKA AHA RR BBA AE RRKK ERE ERKEEA EAE EE 


FUNCTION: main() 
PURPOSE: main program file 
RETURNS: _ resulting 

He HEHE AG HE KC HE EK HE Ke eH KE AC EK i A A OE AE OK oe he 2 OE HE 2 2 a 2 2 2h AC OE 2 2 OR Ag 2 OK EO OE ee OK OK OE OK OK 

*/ 

#include <stdio.h> 

#include <math.h> 

#include "matrix.h" 

#include “list.h" 

#include "terms.h" 

#define pi 3.14159265358979 

int main(void) 

{ 

int data_length; 

printf("Enter the data length"); 

scanf("%d" ,&data_length); 

read_data(data_length); 

initial(-6.0,-6.0,6.0,6.0,50,50); 

initial_kalman(2,0.0,0.0); 
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data_set=cartesian_conv(initialize_data(0,window_length)); 
posest=zeros_matrix(data_set.row,data_set.col); 

potential(); 

slope(data_length); 

return 1; 


} 
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APPENDIX C. PROGRAM LISTINGS FOR CONTROL 
ALGORITHM SIMULATIONS 


A. GENERAL 
This appendix contains the MATLAB files generated for the simulations in 
Chapter III and IV. These files are used in MATLAB SIMULINK simulation models for 
the control system. The constants used are defined in a trial error basis on the occasion of 
the intended trajectory of the vehicle, and will have to be determined for every different 
mission. 
B. PROGRAM LISTINGS 
% To Too To To Yo To Yo Yo To Yo To Yo Yo Yo Yo Yo To Yo Yo To Yo To Yo Yo Yo Yo Yo To Yo To Yo Yo To Yo Yo Yo To Yo Yo Yo 
FILENAME: occont.m 
PURPOSE: MATLAB function giving the outputs of the outer control-loop 
AUTHOR: Hakki Celebioglu 
DATE: 1996 
Jo %o To Mo Yo To Yo Yo Yo Yo Mo Yo To To Yo Yo Mo Yo Yo To Yo Yo To Yo To Yo Yo To To Yo To Yo To Yo To Yo To To To To To Yo 
function uvwstar=occont(xxx) 
% Target positions 
xo=10; 
yo=10; 
x=xxx(1); 9% estimated x-axis vehicle position 
y=xxx(2); 9% estimated y-axis vehicle position 
sie=xxx(3); % orientation of the vehicle with respect to target frame 
e=xxx(4); % the distance between the vehicle and the target frame 
theta=xxx(5); % the angle 6 defined in Chapter II. 
alfa=xxx(6); 9% the angle o defined in Chapter II. 
% The constant denoted for the Long Range Phase 
kLR=0.12; 
%The constant denoted for the Medium Range Phase 
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kMR=0.0173; 
%The constant denoted for Fine Maneuvering Phase 
kF=0.0001; 
% To % Mo To Yo Yo To Yo To oo Yo Yo Yo Yo Yo To Yo Yo Mo Yo Yo Yo To Yo Yo Yo Yo Yo Yo Yo Yo Yo To To Yo Yo To Yo YoYo 
%Long Range Navigation 
if(e>=7.1) 
siestar=atan2(yo-y,x0-Xx); 
ustar=-kLR*(x*cos(siestar)+y*sin(siestar)); 
vstar=0; 
wstar=0; 
uvwstar=[ustar vstar wstar]; 
end 
% Medium Range Navigation 
if(e<7.1 & e>=1) 
ustar=kMR*e*cos(alfa); 
vstar=0; 
wstar=.01 *alfa+kMR*(cos(alfa)*sinc(alfa/pi)*(alfa-1.26*theta)); 
uvwstar=[ustar vstar wstar]; 
end 
% Fine Maneuvering 
if(e<1) 
ustar=-kF*(x*cos(sie)+y*sin(sie)); 
vstar=-kF*(-x*sin(sie)+y*cos(sie)); 
wstar=-0.8*sie; % here gamma is 0.8. 
uvwstar=[ustar vstar wstar]; 


end 
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Jo % To Yo To Yo Yo To Yo Yo To Yo Yo Wo Yo Yo Yo To Yo Yo Yo To Yo Yo To Yo Yo Yo Yo To Yo To Yo Yo Yo To Yo Yo Yo Yo Yo Yo 
FILENAME: iccont.m 
PURPOSE: MATLAB function giving the outputs of the inner control-loop 
AUTHOR: Hakki Celebioglu 
DATE: 1996 

Jo % To Yo To Yo To Yo To Yo Mo Yo Mo Yo Yo Yo Yo To Yo Yo Yo Yo Yo Yo To Yo Yo Yo Yo To Yo To Yo Yo Yo To Yo Yo Yo Yo Yo Yo 

function fgM=iccont(stars) 

% Outputs of the outer control loop will be inputs for inner control loop 

ustar=stars(1); 

vstar=stars(2); 

wstar=stars(3); 

% The linear and angular velocities 

u=stars(7); 

v=stars(8); 

w=stars(9); 

% Time drivatives of the output of the outer control loop 

ustard=stars(4); 

vstard=stars(5); 

wstard=stars(6); 

%Inner Control Loop adjusting parameters 

px=0.1; 

py=0.04; 

pm=0.001; 

Jo To % To To Vo Yo Yo Vo Yo Yo Yo To To To Yo Yo To Yo Yo Yo Yo To Yo Yo Yo To Yo To To Yo Yo To Yo Yo Yo Yo Yo To To Yo Yo 

% The lateral trusts and the angular moment being computed | 

f=-v*w-+ustard-px*(u-ustar); 

g=u*w-+vstard-py*(v-vstar); 

M=wstard-pm*(w-wstar); 


foM=[f g Muv w]; % The computed forces and the velocities will be outputs. 
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% To Vo To Foo To Yo Yo Yo Yo Yo Yo To Jo Yo Yo Yo Yo Yo Yo Yo Yo Mo Yo To Yo Yo Yo Yo Yo Yo Yo To Yo To To To To To Yo Yo 
FILENAME: dinamic.m 
PURPOSE: MATLAB function for dynamics of the vehicle 
AUTHOR: Hakki Celebioglu 
DATE: 1996 
% To To To Fo Jo Jo To To Yo Yo Mo To Yo Yo Yo Yo Yo Mo Yo Yo Yo Mo Yo Yo Yo Yo Yo Yo Yo Yo Yo Yo To To Yo To To To Yo Yo Yo 
function uvw=dinamic(xxx) 
% the outputs of the inner control loop will be inputs for vehicle's dynamics. 
f=xxx(1); 
g=xxx(2); 
M=xxx(3); 
u=xxx(4); 
V=xxXx(5); 
w=xxx(6); 
%The state space parameters of the vehicle's dynamics 
ud=f+v*w; 
vd=g-u*w; 
wd=M; 
uvw=[ud vd wd]; 
% To To Jo To To Mo Yo To Yo Yo Yo Mo Yo Yo Jo Yo Yo Mo To To To Yo Yo Yo Yo To To Yo Yo Yo Yo To Yo To Yo Yo Yo Yo To Yo To 
FILENAME: xysie.m 
PURPOSE: MATLAB function for kinematics of the vehicle 
AUTHOR:  Hakki Celebioglu 
DATE : 1996 
% To To To To To Wo Vo Yo Yo To To Yo Mo Yo To Yo Yo Yo Yo Yo Yo Mo To To To To To Yo To Yo To To Yo Yo Yo To To Yo Yo To To 
function xysie=scont(xxx) 
% Second set of system states and system kinematics 
u=xxx(1); 


V=xxx(2); 
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w=xxx(3); 

sie=xxx(4); 

alfa=xxx(5); 

e=xxx(6); 
xd=u*cos(sie)-v*sin(sie); 
yd=u*sin(sie)+v*cos(sie); 
sied=w; 

_ ed=-u*cos(alfa)-v*sin(alfa); 
thetad=(u*sin(alfa)/e)-(v*cos(alfa)/e); 
alfad=-w-+thetad; 
xysie=[xd yd sied ed thetad alfad]; 
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